#!/usr/bin/env python
# -*- coding: utf-8 -*-
import copy
import argparse
from cmath import pi
import math
from turtle import left

import cv2 as cv
import numpy as np
import mediapipe as mp
import serial
import serial.tools.list_ports 

from utils import CvFpsCalc
left_hand=[0,0]
right_hand=[0,0]
left_shurder=[0,0]
right_shurder=[0,0]

left_waist=[0,0]
right_waist=[0,0]
left_knee=[0,0]
right_knee=[0,0]
cam_width=500
cam_height=400


L=120.0  #连杆长度 单位mm
OC=60.0  #两个舵机旋转轴距离 mm
W=422.5  #左右舵机距离 mm
py_left2right=210.0  #皮影臂展长度  mm
leg_max=40 #腿部最大开合角度 度

step_count=1600  #步进电机转一圈 需要1600个脉冲   行程是60mm
step_one=60
step_length=360  #步进电机行程 36cm  360mm
step_allcount=(step_length/step_one)*1600
step_x=0.5*step_allcount  # 步进电机 默认位置为正中心 
step_error=10



#连接CH340串口芯片的ARDUINO
try:
    com_index=""
    port_list = list(serial.tools.list_ports.comports())
    for p in port_list: 
        if (p.pid==29987 and p.vid == 6790):
            print("发现目标Arduino："+str(p.description))
            com_index=str(p.device)
    baudRate=19200  #波特率
    ser=serial.Serial(com_index,baudRate)
    print("arduino已连接")
except:
    print("请检查USB ardunio连接")


#步进电机控制函数
def step_to(x):
  global step_x
  step_t=int((x/cam_width)*step_allcount)
  step_x_error=step_t-step_x
  if abs(step_x_error)>step_error:
    ser.write(("x"+str(step_x_error)).encode())
    step_x=step_t

#设置左手的两个舵机角度
def set_left_xy(x, y):
    try:
        KO=math.sqrt(x**2+y**2)
        #print("KO:",KO)
        alpha1=math.atan(x/y)
        if (alpha1<0):
            alpha1=pi+alpha1
        #print("alpha1:",alpha1*180/pi)
        alpha2=math.acos((KO**2)/(2*L*KO))
        #print("alpha2:",alpha2*180/pi)
        alpha=min(int((alpha1+alpha2)*180/pi),180)
        print("a:",alpha)

        #beta1=math.acos((2*L**2-KO**2)/(2*L**2))
        #print("beta1:",beta1)
        KC=abs(math.sqrt(OC**2+KO**2-2*KO*OC*math.cos(alpha1)))
        #print("KC:",KC)
        #theta1=math.acos((OC**2+KC**2-KO**2)/(2*OC*KC))
        theta1=math.atan(x/(OC-y))
        if (theta1<0):
            theta1=pi+theta1
        #print("theta1:",theta1*180/pi)
        theta2=math.acos((KC**2)/(2*KC*L))
        theta=min(int((pi-theta1-theta2)*180/pi),180)
        print("b: ",theta)
        ser.write(("a"+str(alpha)).encode())
        ser.write(("b"+str(theta)).encode())
    except:
        print("数值不可计算")

#设置右手的两个舵机角度
def set_right_xy(x, y):
    try:
        KO=math.sqrt(x**2+y**2)
        #print("KO:",KO)
        alpha1=math.atan((-x/y))
        if (alpha1<0):
            alpha1=pi+alpha1
        #print("alpha1:",alpha1*180/pi)
        alpha2=abs(math.acos((KO**2)/(2*L*KO)))
        #print("alpha2:",alpha2*180/pi)
        alpha=min(int((pi-alpha1-alpha2)*180/pi),180)
        print("f:",alpha)

        #beta1=math.acos((2*L**2-KO**2)/(2*L**2))
        #print("beta1:",beta1)
        KC=abs(math.sqrt(OC**2+KO**2-2*KO*OC*math.cos(alpha1)))
        #print("KC:",KC)
        #theta1=math.acos((OC**2+KC**2-KO**2)/(2*OC*KC))
        theta1=math.atan(-x/(OC-y))
        if (theta1<0):
            theta1=pi+theta1
        #print("theta1:",theta1*180/pi)
        theta2=abs(math.acos((KC**2)/(2*KC*L)))
        #print("theta2:",theta2*180/pi)
        theta=min(int((theta1+theta2)*180/pi),180)
        print("e: ",theta)
        ser.write(("f"+str(alpha)).encode())
        ser.write(("e"+str(theta)).encode())
    except:
        print("数值不可计算")

#设置左腿角度
def set_left_leg(ld):
    try:
        max_ld=min(leg_max,ld)
        left_leg=int(90-max_ld)
        print("左腿角度c:",left_leg)
        ser.write(("c"+str(left_leg)).encode())
    except:
        print("数值不可计算")

#设置右腿角度
def set_right_leg(rd):
    try:
        max_rd=min(leg_max,rd)
        right_leg=int(90+max_rd)
        print("右腿角度d：",right_leg)
        ser.write(("d"+str(right_leg)).encode())
    except:
        print("数值不可计算")


def get_args():
    parser = argparse.ArgumentParser()

    parser.add_argument("--device", type=int, default=1)
    parser.add_argument("--width", help='cap width', type=int, default=cam_width) # 960
    parser.add_argument("--height", help='cap height', type=int, default=cam_height) #1540

    # parser.add_argument('--upper_body_only', action='store_true')  # 0.8.3 or less
    parser.add_argument("--model_complexity",
                        help='model_complexity(0,1(default),2)',
                        type=int,
                        default=1)
    parser.add_argument("--min_detection_confidence",
                        help='min_detection_confidence',
                        type=float,
                        default=0.5)
    parser.add_argument("--min_tracking_confidence",
                        help='min_tracking_confidence',
                        type=float,
                        default=0.5)
    parser.add_argument('--enable_segmentation', action='store_true')
    parser.add_argument("--segmentation_score_th",
                        help='segmentation_score_threshold',
                        type=float,
                        default=0.5)

    parser.add_argument('--use_brect', action='store_true')
    parser.add_argument('--plot_world_landmark', action='store_true')

    args = parser.parse_args()

    return args


def main():
    # 引数解析 #################################################################
    args = get_args()

    cap_device = args.device
    cap_width = args.width
    cap_height = args.height

    #upper_body_only = args.upper_body_only
    model_complexity = args.model_complexity
    min_detection_confidence = args.min_detection_confidence
    min_tracking_confidence = args.min_tracking_confidence
    enable_segmentation = args.enable_segmentation
    segmentation_score_th = args.segmentation_score_th

    use_brect = args.use_brect
    plot_world_landmark = args.plot_world_landmark

    # カメラ準備 ###############################################################
    try:
        cap = cv.VideoCapture(cap_device,cv.CAP_DSHOW)
        cap.set(cv.CAP_PROP_FRAME_WIDTH, cap_width)
        cap.set(cv.CAP_PROP_FRAME_HEIGHT, cap_height)
    except:
        print("请检查摄像头连接！")

    # モデルロード #############################################################
    mp_pose = mp.solutions.pose
    pose = mp_pose.Pose(
        # upper_body_only=upper_body_only,
        model_complexity=model_complexity,
        enable_segmentation=enable_segmentation,
        min_detection_confidence=min_detection_confidence,
        min_tracking_confidence=min_tracking_confidence,
    )

    # FPS計測モジュール ########################################################
    cvFpsCalc = CvFpsCalc(buffer_len=10)

    # World座標プロット ########################################################
    if plot_world_landmark:
        import matplotlib.pyplot as plt
        
        fig = plt.figure()
        ax = fig.add_subplot(111, projection="3d")
        fig.subplots_adjust(left=0.0, right=1, bottom=0, top=1)

    while True:
        display_fps = cvFpsCalc.get()

        # カメラキャプチャ #####################################################
        ret, image = cap.read()
        if not ret:
            break
        image = cv.flip(image, 1)  # ミラー表示
        debug_image = copy.deepcopy(image)

        # 検出実施 #############################################################
        image = cv.cvtColor(image, cv.COLOR_BGR2RGB)
        results = pose.process(image)

        # 描画 ################################################################
        if enable_segmentation and results.segmentation_mask is not None:
            # セグメンテーション
            mask = np.stack((results.segmentation_mask, ) * 3,
                            axis=-1) > segmentation_score_th
            bg_resize_image = np.zeros(image.shape, dtype=np.uint8)
            bg_resize_image[:] = (0, 255, 0)
            debug_image = np.where(mask, debug_image, bg_resize_image)
        if results.pose_landmarks is not None:
            # 外接矩形の計算
            brect = calc_bounding_rect(debug_image, results.pose_landmarks)
            # 描画
            debug_image = draw_landmarks(
                debug_image,
                results.pose_landmarks,
                # upper_body_only,
            )
            debug_image = draw_bounding_rect(use_brect, debug_image, brect)

        # World座標プロット ###################################################
        if plot_world_landmark:
            if results.pose_world_landmarks is not None:
                plot_world_landmarks(
                    plt,
                    ax,
                    results.pose_world_landmarks,
                )

        # FPS表示
        if enable_segmentation and results.segmentation_mask is not None:
            fps_color = (255, 255, 255)
        else:
            fps_color = (0, 255, 0)
        cv.putText(debug_image, "FPS:" + str(display_fps), (10, 30),
                   cv.FONT_HERSHEY_SIMPLEX, 1.0, fps_color, 2, cv.LINE_AA)

        # キー処理(ESC：終了) #################################################
        key = cv.waitKey(1)
        if key == 27:  # ESC
            break

        # 画面反映 #############################################################
        cv.imshow('MediaPipe Pose Demo', debug_image)

    cap.release()
    cv.destroyAllWindows()


def calc_bounding_rect(image, landmarks):
    image_width, image_height = image.shape[1], image.shape[0]

    landmark_array = np.empty((0, 2), int)

    for _, landmark in enumerate(landmarks.landmark):
        landmark_x = min(int(landmark.x * image_width), image_width - 1)
        landmark_y = min(int(landmark.y * image_height), image_height - 1)

        landmark_point = [np.array((landmark_x, landmark_y))]

        landmark_array = np.append(landmark_array, landmark_point, axis=0)

    x, y, w, h = cv.boundingRect(landmark_array)

    return [x, y, x + w, y + h]


def draw_landmarks(
    image,
    landmarks,
    # upper_body_only,
    visibility_th=0.5,
):
    image_width, image_height = image.shape[1], image.shape[0]

    landmark_point = []
    left_hand=[]
    right_hand=[]
    left_shurder=[]
    right_shurder=[]
    left_knee=[]
    right_knee=[]

    for index, landmark in enumerate(landmarks.landmark):
        landmark_x = min(int(landmark.x * image_width), image_width - 1)
        landmark_y = min(int(landmark.y * image_height), image_height - 1)
        landmark_z = landmark.z
        landmark_point.append([landmark.visibility, (landmark_x, landmark_y)])

        if landmark.visibility < visibility_th:
            continue

        if index == 0:  # 鼻
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 1:  # 右目：目頭
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 2:  # 右目：瞳
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 3:  # 右目：目尻
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 4:  # 左目：目頭
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 5:  # 左目：瞳
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 6:  # 左目：目尻
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 7:  # 右耳
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 8:  # 左耳
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 9:  # 口：左端
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 10:  # 口：左端
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 11:  # 右肩-------------------
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 0, 255), 3)
            right_shurder=[landmark_x, landmark_y]
        if index == 12:  # 左肩--------------------------
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 0, 255), 3)
            left_shurder=[landmark_x, landmark_y]
        if index == 13:  # 右肘
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 14:  # 左肘
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 15:  # 右手首
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 16:  # 左手首
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 17:  # 右手1(外側端)  ---------------------
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 0, 255), 3)
            right_hand=[landmark_x, landmark_y]
        if index == 18:  # 左手1(外側端) ---------------------
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 0, 255), 3)
            left_hand=[landmark_x, landmark_y]
        if index == 19:  # 右手2(先端)
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 20:  # 左手2(先端)
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 21:  # 右手3(内側端)
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 22:  # 左手3(内側端)
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 23:  # 腰(右側)
            cv.circle(image, (landmark_x, landmark_y), 5, (255, 0, 0), 5)
            right_waist=[landmark_x,landmark_y]
        if index == 24:  # 腰(左側)
            cv.circle(image, (landmark_x, landmark_y), 5, (255, 0, 0), 5)
            left_waist=[landmark_x,landmark_y]
        if index == 25:  # 右ひざ
            cv.circle(image, (landmark_x, landmark_y), 5, (255, 0, 0), 2)
            right_knee=[landmark_x,landmark_y]
        if index == 26:  # 左ひざ
            cv.circle(image, (landmark_x, landmark_y), 5, (255, 0, 0), 2)
            left_knee=[landmark_x,landmark_y]
        if index == 27:  # 右足首
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 28:  # 左足首
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 29:  # 右かかと
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 30:  # 左かかと
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 31:  # 右つま先
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)
        if index == 32:  # 左つま先
            cv.circle(image, (landmark_x, landmark_y), 5, (0, 255, 0), 2)


        # if not upper_body_only:
        if True:
            cv.putText(image, "z:" + str(round(landmark_z, 3)),
                       (landmark_x - 10, landmark_y - 10),
                       cv.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1,
                       cv.LINE_AA)
        if (len(left_shurder)>0 and len(right_shurder)>0):
            jk=math.sqrt((right_shurder[0]-left_shurder[0])**2+(right_shurder[1]-left_shurder[1])**2)
            k=py_left2right/(4.5*jk)  #画面对于实际距离的比例系数
            #print("jk:",jk)
            middle_shuder=[(right_shurder[0]+left_shurder[0])/2,(right_shurder[1]+left_shurder[1])/2]
            #print("middle:",middle_shuder)
            step_to(middle_shuder[0])

            if (len(left_hand)>0):
                x1=k*(left_hand[0]-middle_shuder[0])+W/2
                #print(left_hand[0]-middle_shuder[0])
                y1=k*(left_hand[1]-middle_shuder[1])+OC/2
                if (x1>(W/2-110) and x1<(W/2) and y1>(OC/2-110) and y1<(OC/2+110)):
                    print("x1",x1)                   
                    print("y1",y1)
                    set_left_xy(x1,y1)
                else:
                    print("x1y1左手超出边界")
                    
            if (len(right_hand)>0):
                x2=k*(right_hand[0]-middle_shuder[0])-W/2
                #print(left_hand[0]-middle_shuder[0])
                y2=k*(right_hand[1]-middle_shuder[1])+OC/2
                if(x2>(-W)/2 and x2<((-W)/2+110) and y2>(OC/2-110) and y2<(OC/2+110)):        
                    print("x2",x2)
                    print("y2",y2)
                    set_right_xy(x2,y2)
                else:
                    print("x2y2右手超出边界")
        if (len(left_knee)>0 and len(left_waist)>0):
            dx=abs(left_knee[0]-left_waist[0])
            dy=abs(left_knee[1]-left_waist[1])
            if( dx>0 and dy>0):
                left_knee_degree=int(math.atan(float(dx)/float(dy))*180/pi)
                #print("左腿角度:",left_knee_degree)
                set_left_leg(left_knee_degree)
        
        if (len(right_knee)>0 and len(right_waist)>0):
            dx=abs(right_knee[0]-right_waist[0])
            dy=abs(right_knee[1]-right_waist[1])
            if (dx>0 and dy>0):
                right_knee_degree=int(math.atan(float(dx)/float(dy))*180/pi)
                #print("右腿角度:",right_knee_degree)
                set_right_leg(right_knee_degree)

            # pymin=(int(max((middle_shuder[0]-2.5*jk),0)),int(max((middle_shuder[1]-2*jk),0)))
            # print(pymin)
            # pymax=(int(min((middle_shuder[0]+2.5*jk),cam_width)),int(min((middle_shuder[1]+3*jk),cam_height)))
            # cv.rectangle(image,pymin,pymax,(0,0,255),5)
            # left_shurder=[]
            # right_shurder=[]

    if len(landmark_point) > 0:
        # 右目
        if landmark_point[1][0] > visibility_th and landmark_point[2][
                0] > visibility_th:
            cv.line(image, landmark_point[1][1], landmark_point[2][1],
                    (0, 255, 0), 2)
        if landmark_point[2][0] > visibility_th and landmark_point[3][
                0] > visibility_th:
            cv.line(image, landmark_point[2][1], landmark_point[3][1],
                    (0, 255, 0), 2)

        # 左目
        if landmark_point[4][0] > visibility_th and landmark_point[5][
                0] > visibility_th:
            cv.line(image, landmark_point[4][1], landmark_point[5][1],
                    (0, 255, 0), 2)
        if landmark_point[5][0] > visibility_th and landmark_point[6][
                0] > visibility_th:
            cv.line(image, landmark_point[5][1], landmark_point[6][1],
                    (0, 255, 0), 2)

        # 口
        if landmark_point[9][0] > visibility_th and landmark_point[10][
                0] > visibility_th:
            cv.line(image, landmark_point[9][1], landmark_point[10][1],
                    (0, 255, 0), 2)

        # 肩
        if landmark_point[11][0] > visibility_th and landmark_point[12][
                0] > visibility_th:
            cv.line(image, landmark_point[11][1], landmark_point[12][1],
                    (0, 255, 0), 2)

        # 右腕
        if landmark_point[11][0] > visibility_th and landmark_point[13][
                0] > visibility_th:
            cv.line(image, landmark_point[11][1], landmark_point[13][1],
                    (0, 255, 0), 2)
        if landmark_point[13][0] > visibility_th and landmark_point[15][
                0] > visibility_th:
            cv.line(image, landmark_point[13][1], landmark_point[15][1],
                    (0, 255, 0), 2)

        # 左腕
        if landmark_point[12][0] > visibility_th and landmark_point[14][
                0] > visibility_th:
            cv.line(image, landmark_point[12][1], landmark_point[14][1],
                    (0, 255, 0), 2)
        if landmark_point[14][0] > visibility_th and landmark_point[16][
                0] > visibility_th:
            cv.line(image, landmark_point[14][1], landmark_point[16][1],
                    (0, 255, 0), 2)

        # 右手
        if landmark_point[15][0] > visibility_th and landmark_point[17][
                0] > visibility_th:
            cv.line(image, landmark_point[15][1], landmark_point[17][1],
                    (0, 255, 0), 2)
        if landmark_point[17][0] > visibility_th and landmark_point[19][
                0] > visibility_th:
            cv.line(image, landmark_point[17][1], landmark_point[19][1],
                    (0, 255, 0), 2)
        if landmark_point[19][0] > visibility_th and landmark_point[21][
                0] > visibility_th:
            cv.line(image, landmark_point[19][1], landmark_point[21][1],
                    (0, 255, 0), 2)
        if landmark_point[21][0] > visibility_th and landmark_point[15][
                0] > visibility_th:
            cv.line(image, landmark_point[21][1], landmark_point[15][1],
                    (0, 255, 0), 2)

        # 左手
        if landmark_point[16][0] > visibility_th and landmark_point[18][
                0] > visibility_th:
            cv.line(image, landmark_point[16][1], landmark_point[18][1],
                    (0, 255, 0), 2)
        if landmark_point[18][0] > visibility_th and landmark_point[20][
                0] > visibility_th:
            cv.line(image, landmark_point[18][1], landmark_point[20][1],
                    (0, 255, 0), 2)
        if landmark_point[20][0] > visibility_th and landmark_point[22][
                0] > visibility_th:
            cv.line(image, landmark_point[20][1], landmark_point[22][1],
                    (0, 255, 0), 2)
        if landmark_point[22][0] > visibility_th and landmark_point[16][
                0] > visibility_th:
            cv.line(image, landmark_point[22][1], landmark_point[16][1],
                    (0, 255, 0), 2)

        # 胴体
        if landmark_point[11][0] > visibility_th and landmark_point[23][
                0] > visibility_th:
            cv.line(image, landmark_point[11][1], landmark_point[23][1],
                    (0, 255, 0), 2)
        if landmark_point[12][0] > visibility_th and landmark_point[24][
                0] > visibility_th:
            cv.line(image, landmark_point[12][1], landmark_point[24][1],
                    (0, 255, 0), 2)
        if landmark_point[23][0] > visibility_th and landmark_point[24][
                0] > visibility_th:
            cv.line(image, landmark_point[23][1], landmark_point[24][1],
                    (0, 255, 0), 2)

        if len(landmark_point) > 25:
            # 右足
            if landmark_point[23][0] > visibility_th and landmark_point[25][
                    0] > visibility_th:
                cv.line(image, landmark_point[23][1], landmark_point[25][1],
                        (0, 255, 0), 2)
            if landmark_point[25][0] > visibility_th and landmark_point[27][
                    0] > visibility_th:
                cv.line(image, landmark_point[25][1], landmark_point[27][1],
                        (0, 255, 0), 2)
            if landmark_point[27][0] > visibility_th and landmark_point[29][
                    0] > visibility_th:
                cv.line(image, landmark_point[27][1], landmark_point[29][1],
                        (0, 255, 0), 2)
            if landmark_point[29][0] > visibility_th and landmark_point[31][
                    0] > visibility_th:
                cv.line(image, landmark_point[29][1], landmark_point[31][1],
                        (0, 255, 0), 2)

            # 左足
            if landmark_point[24][0] > visibility_th and landmark_point[26][
                    0] > visibility_th:
                cv.line(image, landmark_point[24][1], landmark_point[26][1],
                        (0, 255, 0), 2)
            if landmark_point[26][0] > visibility_th and landmark_point[28][
                    0] > visibility_th:
                cv.line(image, landmark_point[26][1], landmark_point[28][1],
                        (0, 255, 0), 2)
            if landmark_point[28][0] > visibility_th and landmark_point[30][
                    0] > visibility_th:
                cv.line(image, landmark_point[28][1], landmark_point[30][1],
                        (0, 255, 0), 2)
            if landmark_point[30][0] > visibility_th and landmark_point[32][
                    0] > visibility_th:
                cv.line(image, landmark_point[30][1], landmark_point[32][1],
                        (0, 255, 0), 2)
    return image


def plot_world_landmarks(
    plt,
    ax,
    landmarks,
    visibility_th=0.5,
):
    landmark_point = []

    for index, landmark in enumerate(landmarks.landmark):
        landmark_point.append(
            [landmark.visibility, (landmark.x, landmark.y, landmark.z)])

    face_index_list = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
    right_arm_index_list = [11, 13, 15, 17, 19, 21]
    left_arm_index_list = [12, 14, 16, 18, 20, 22]
    right_body_side_index_list = [11, 23, 25, 27, 29, 31]
    left_body_side_index_list = [12, 24, 26, 28, 30, 32]
    shoulder_index_list = [11, 12]
    waist_index_list = [23, 24]

    # 顔
    face_x, face_y, face_z = [], [], []
    for index in face_index_list:
        point = landmark_point[index][1]
        face_x.append(point[0])
        face_y.append(point[2])
        face_z.append(point[1] * (-1))

    # 右腕
    right_arm_x, right_arm_y, right_arm_z = [], [], []
    for index in right_arm_index_list:
        point = landmark_point[index][1]
        right_arm_x.append(point[0])
        right_arm_y.append(point[2])
        right_arm_z.append(point[1] * (-1))

    # 左腕
    left_arm_x, left_arm_y, left_arm_z = [], [], []
    for index in left_arm_index_list:
        point = landmark_point[index][1]
        left_arm_x.append(point[0])
        left_arm_y.append(point[2])
        left_arm_z.append(point[1] * (-1))

    # 右半身
    right_body_side_x, right_body_side_y, right_body_side_z = [], [], []
    for index in right_body_side_index_list:
        point = landmark_point[index][1]
        right_body_side_x.append(point[0])
        right_body_side_y.append(point[2])
        right_body_side_z.append(point[1] * (-1))

    # 左半身
    left_body_side_x, left_body_side_y, left_body_side_z = [], [], []
    for index in left_body_side_index_list:
        point = landmark_point[index][1]
        left_body_side_x.append(point[0])
        left_body_side_y.append(point[2])
        left_body_side_z.append(point[1] * (-1))

    # 肩
    shoulder_x, shoulder_y, shoulder_z = [], [], []
    for index in shoulder_index_list:
        point = landmark_point[index][1]
        shoulder_x.append(point[0])
        shoulder_y.append(point[2])
        shoulder_z.append(point[1] * (-1))

    # 腰
    waist_x, waist_y, waist_z = [], [], []
    for index in waist_index_list:
        point = landmark_point[index][1]
        waist_x.append(point[0])
        waist_y.append(point[2])
        waist_z.append(point[1] * (-1))
            
    ax.cla()
    ax.set_xlim3d(-1, 1)
    ax.set_ylim3d(-1, 1)
    ax.set_zlim3d(-1, 1)

    ax.scatter(face_x, face_y, face_z)
    ax.plot(right_arm_x, right_arm_y, right_arm_z)
    ax.plot(left_arm_x, left_arm_y, left_arm_z)
    ax.plot(right_body_side_x, right_body_side_y, right_body_side_z)
    ax.plot(left_body_side_x, left_body_side_y, left_body_side_z)
    ax.plot(shoulder_x, shoulder_y, shoulder_z)
    ax.plot(waist_x, waist_y, waist_z)
    
    plt.pause(.001)

    return


def draw_bounding_rect(use_brect, image, brect):
    if use_brect:
        # 外接矩形
        cv.rectangle(image, (brect[0], brect[1]), (brect[2], brect[3]),
                     (0, 255, 0), 2)

    return image


if __name__ == '__main__':
    main()
